feat(ndt_scan_matcher): estimate NDT covariance in real-time with limited initial positions #5338

KOKIAOKI · 2023-10-17T16:08:19Z

Description

Calculate 2D covariance (xx, xy, yx, yy) in real time using the convergence of NDT.
I limit the arrangement of multiple initial positions based on the Hessian matrix of the NDT score function.

Tests performed

I evaluated it In a tunnel evaluation (already shared in weekly meeting).
The real-time covariance estimation obtained the same convergence tendency as offline convergence evaluation.

Notes for reviewers

Related PRs:

feat: output Hessian matrix tier4/ndt_omp#22
Output Hessian matrix
feat(ndt_scan_matcher): add parameters of real-time covariance estimation autoware_launch#643
Parameters for this covariance estaimtion

Interface changes

topic
/localization/pose_estimator/multi_ndt_pose
->NDT convergence positions
/localization/pose_estimator/multi_initial_pose
->NDT initial positions
yaml setting (ndt_scan_matcher.param.yaml)
Category: 2D Real-time covariance estimation (output_pose_covariance is the minimum value)
Default use_covariance_estimation is false.

Effects on system behavior

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

I've confirmed the [contribution guidelines].
The PR follows the [pull request guidelines].

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

The PR follows the [pull request guidelines].
The PR has been properly tested.
The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

There are no open discussions or they are tracked via tickets.
The PR is ready for merge.
After all checkboxes are checked, anyone who has write access can merge the PR.
[contribution guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/
[pull request guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/pull-request-guidelines/

KYabuuchi

Thank you for creating the PR. I found a risky code in a brief review. Please check.

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

Initialize Eigen::Vector2d Co-authored-by: Kento Yabuuchi <[email protected]>

kminoda

Hi, thank you for the PR 🎉
Left some minor comments so please check 🙏

kminoda · 2023-10-18T04:46:18Z

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -141,6 +141,15 @@ NDTScanMatcher::NDTScanMatcher()
  initial_pose_distance_tolerance_m_ =
    this->declare_parameter<double>("initial_pose_distance_tolerance_m");

+  use_cov_estimation_ = this->declare_parameter("use_covariance_estimation", use_cov_estimation_);


Please do not use the default parameter and instead do as follow

Suggested change

use_cov_estimation_ = this->declare_parameter("use_covariance_estimation", use_cov_estimation_);

use_cov_estimation_ = this->declare_parameter<bool>("use_covariance_estimation");

kminoda · 2023-10-18T04:47:32Z

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

+  // covariance estimation
+  std::array<double, 36> ndt_covariance;
+  ndt_covariance = output_pose_covariance_;
+  if (is_converged && use_cov_estimation_) {
+    estimate_covariance(ndt_result, initial_pose_matrix, ndt_covariance, sensor_ros_time);
+  }


I believe this part is relatively computationally heavy, so I would recommend to move this right after the following part:

// perform ndt scan matching (*state_ptr_)["state"] = "Aligning"; const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(interpolator.get_current_pose().pose.pose); auto output_cloud = std::make_shared<pcl::PointCloud<PointSource>>(); ndt_ptr_->align(*output_cloud, initial_pose_matrix); const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); (*state_ptr_)["state"] = "Sleeping

This will also enable users to monitor the total execution time with exe_time as well.

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

YamatoAndo · 2023-10-18T05:26:40Z

@KOKIAOKI Thank you for your contribution!
Could you write an explanation in the readme?
I'd also like you to include a link of your paper in the readme.

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

KOKIAOKI · 2023-10-18T08:27:14Z

Thank you for checking my implementation.
Please let me test again after fixing these fixes.

Signed-off-by: Koki Aoki <[email protected]>

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

KOKIAOKI · 2023-10-24T09:04:03Z

I tested the fixed source code. It worked fine in my environment.

Here are the corrections

I have corrected all the issues pointed out by reviewers.
- Moved is_converged processing and processing time calculation.
Added "const" to all immutable variables.
Removed unnecessary comments

I will resolve the confliction later

KYabuuchi

I added some comments. Please check it out 🙏

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

localization/ndt_scan_matcher/README.md

localization/localization_util/src/util_func.cpp

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

… process Signed-off-by: Koki Aoki <[email protected]>

Signed-off-by: Koki Aoki <[email protected]>

KOKIAOKI · 2023-10-26T08:12:19Z

Sorry for the delay in reporting.
I have reflected all the reviews made by @Motsu-san and @KYabuuchi into the source code.

Changed the calculation of covariance to a method that uses smaller variables.
There were many variable names that caused misunderstandings, so I made them easier to understand.
Changed and added notes and explanations in README

Thank you for further reviewing!

codecov · 2023-10-27T06:16:15Z

Codecov Report

Attention: 65 lines in your changes are missing coverage. Please review.

Comparison is base (4e00bf2) 14.84% compared to head (3e31b93) 15.09%.
Report is 2 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5338      +/-   ##
==========================================
+ Coverage   14.84%   15.09%   +0.24%     
==========================================
  Files        1665     1661       -4     
  Lines      116231   115372     -859     
  Branches    36256    35646     -610     
==========================================
+ Hits        17259    17419     +160     
+ Misses      79473    78436    -1037     
- Partials    19499    19517      +18

Flag	Coverage Δ		*Carryforward flag
differential	`0.00% <0.00%> (?)`
total	`15.10% <ø> (+0.25%)`	⬆️	Carriedforward from 5eda6c3

*This pull request uses carry forward flags. Click here to find out more.

Files	Coverage Δ
...ion/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp	`0.00% <0.00%> (ø)`

... and 64 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

localization/ndt_scan_matcher/README.md

Signed-off-by: Koki Aoki <[email protected]>

KOKIAOKI · 2023-10-31T07:07:09Z

@YamatoAndo @KYabuuchi
Thank you for further checks.
I created the function of eigenvector calculation and added README empty line.

localization/localization_util/include/localization_util/util_func.hpp

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

localization/localization_util/src/util_func.cpp

localization/ndt_scan_matcher/README.md

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

…to ndt_scan_matcher.cpp, fix README line Signed-off-by: Koki Aoki <[email protected]>

KOKIAOKI · 2023-11-01T05:42:55Z

@YamatoAndo @KYabuuchi @SakodaShintaro
Thank you for further code checks.
Main changes are as below:

@SakodaShintaro Like your advice, I added parameter warning and std::exeption about Eigen::Success.
@SakodaShintaro Although review about util.cpp was closed, I moved two functions from util.cpp to ndt_scan_matcher.cpp.

localization/ndt_scan_matcher/README.md

KOKIAOKI added 6 commits August 3, 2023 14:10

add covariance estiamtion

711b821

fix: msg

7025f9e

fix: typo

50180f3

fix: yaml

6444d2f

fix

130a008

Merge remote-tracking branch 'upstream/main'

f391d75

KOKIAOKI requested review from KYabuuchi, kminoda, Motsu-san and YamatoAndo as code owners October 17, 2023 16:08

github-actions bot added the component:localization Vehicle's position determination in its environment. (auto-assigned) label Oct 17, 2023

KOKIAOKI mentioned this pull request Oct 17, 2023

feat(ndt_scan_matcher): add parameters of real-time covariance estimation autowarefoundation/autoware_launch#643

Merged

7 tasks

KYabuuchi reviewed Oct 18, 2023

View reviewed changes

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Outdated Show resolved Hide resolved

Update localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

f20bee4

Initialize Eigen::Vector2d Co-authored-by: Kento Yabuuchi <[email protected]>

kminoda reviewed Oct 18, 2023

View reviewed changes

YamatoAndo reviewed Oct 18, 2023

View reviewed changes

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Outdated Show resolved Hide resolved

fix: vector initilization, add: explanation in README

692625f

Signed-off-by: Koki Aoki <[email protected]>

github-actions bot added the type:documentation Creating or refining documentation. (auto-assigned) label Oct 18, 2023

Merge branch 'main' into main

dc63ba0

YamatoAndo reviewed Oct 19, 2023

View reviewed changes

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Outdated Show resolved Hide resolved

YamatoAndo reviewed Oct 19, 2023

View reviewed changes

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Outdated Show resolved Hide resolved

YamatoAndo reviewed Oct 19, 2023

View reviewed changes

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Show resolved Hide resolved

KOKIAOKI and others added 2 commits October 23, 2023 11:14

Merge branch 'main' into main

ea68d6b

fix: initialization and some build errors

69f0e2c

Merge remote-tracking branch 'upstream/main'

af03d2d

KYabuuchi reviewed Oct 25, 2023

View reviewed changes

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Outdated Show resolved Hide resolved

KOKIAOKI added 2 commits October 25, 2023 16:26

refactor: variable and function names, changed covariance calculation…

2cf6018

… process Signed-off-by: Koki Aoki <[email protected]>

fix: README explanation

9f6098b

Signed-off-by: Koki Aoki <[email protected]>

Merge branch 'main' into main

dde2a8a

YamatoAndo added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Oct 27, 2023

YamatoAndo reviewed Oct 27, 2023

View reviewed changes

localization/ndt_scan_matcher/README.md Outdated Show resolved Hide resolved

Merge branch 'main' into main

19eb8a5

KOKIAOKI requested a review from SakodaShintaro as a code owner October 31, 2023 03:19

KOKIAOKI and others added 2 commits October 31, 2023 15:53

Merge branch 'main' into main

d129cfb

fix: functionalization of eigenvector calculation, README empty line

41442ad

Signed-off-by: Koki Aoki <[email protected]>

SakodaShintaro reviewed Oct 31, 2023

View reviewed changes

localization/localization_util/include/localization_util/util_func.hpp Outdated Show resolved Hide resolved

SakodaShintaro reviewed Oct 31, 2023

View reviewed changes

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Show resolved Hide resolved

SakodaShintaro reviewed Oct 31, 2023

View reviewed changes

localization/localization_util/src/util_func.cpp Outdated Show resolved Hide resolved

YamatoAndo reviewed Oct 31, 2023

View reviewed changes

localization/ndt_scan_matcher/README.md Show resolved Hide resolved

YamatoAndo reviewed Oct 31, 2023

View reviewed changes

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp Outdated Show resolved Hide resolved

add warning outputs, rename function, move two functions in util.cpp …

468d3ce

…to ndt_scan_matcher.cpp, fix README line Signed-off-by: Koki Aoki <[email protected]>

Merge branch 'main' into main

84a5735

mergify bot mentioned this pull request Nov 1, 2023

feat(ndt_scan_matcher): estimate NDT covariance in real-time with limited initial positions #2 #5454

Closed

7 tasks

YamatoAndo reviewed Nov 1, 2023

View reviewed changes

localization/ndt_scan_matcher/README.md Outdated Show resolved Hide resolved

YamatoAndo reviewed Nov 1, 2023

View reviewed changes

localization/ndt_scan_matcher/README.md Outdated Show resolved Hide resolved

KOKIAOKI and others added 2 commits November 2, 2023 14:00

Merge branch 'main' into main

3bf2b1f

fix: format of markdown

5eda6c3

YamatoAndo reviewed Nov 2, 2023

View reviewed changes

localization/ndt_scan_matcher/README.md Outdated Show resolved Hide resolved

fix: add space in readme

3e31b93

YamatoAndo approved these changes Nov 7, 2023

View reviewed changes

YamatoAndo merged commit 55c615c into autowarefoundation:main Nov 7, 2023
19 of 24 checks passed

tier4-autoware-public-bot bot mentioned this pull request Nov 8, 2023

chore: sync upstream tier4/autoware.universe#1005

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(ndt_scan_matcher): estimate NDT covariance in real-time with limited initial positions #5338

feat(ndt_scan_matcher): estimate NDT covariance in real-time with limited initial positions #5338

KOKIAOKI commented Oct 17, 2023 •

edited

Loading

KYabuuchi left a comment

kminoda left a comment

kminoda Oct 18, 2023

kminoda Oct 18, 2023

YamatoAndo commented Oct 18, 2023

KOKIAOKI commented Oct 18, 2023 •

edited

Loading

KOKIAOKI commented Oct 24, 2023 •

edited

Loading

KYabuuchi left a comment

KOKIAOKI commented Oct 26, 2023 •

edited

Loading

codecov bot commented Oct 27, 2023 •

edited

Loading

KOKIAOKI commented Oct 31, 2023

KOKIAOKI commented Nov 1, 2023

	use_cov_estimation_ = this->declare_parameter("use_covariance_estimation", use_cov_estimation_);
	use_cov_estimation_ = this->declare_parameter<bool>("use_covariance_estimation");

feat(ndt_scan_matcher): estimate NDT covariance in real-time with limited initial positions #5338

feat(ndt_scan_matcher): estimate NDT covariance in real-time with limited initial positions #5338

Conversation

KOKIAOKI commented Oct 17, 2023 • edited Loading

Description

Related links

Tests performed

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

KYabuuchi left a comment

Choose a reason for hiding this comment

kminoda left a comment

Choose a reason for hiding this comment

kminoda Oct 18, 2023

Choose a reason for hiding this comment

kminoda Oct 18, 2023

Choose a reason for hiding this comment

YamatoAndo commented Oct 18, 2023

KOKIAOKI commented Oct 18, 2023 • edited Loading

KOKIAOKI commented Oct 24, 2023 • edited Loading

KYabuuchi left a comment

Choose a reason for hiding this comment

KOKIAOKI commented Oct 26, 2023 • edited Loading

codecov bot commented Oct 27, 2023 • edited Loading

Codecov Report

KOKIAOKI commented Oct 31, 2023

KOKIAOKI commented Nov 1, 2023

KOKIAOKI commented Oct 17, 2023 •

edited

Loading

KOKIAOKI commented Oct 18, 2023 •

edited

Loading

KOKIAOKI commented Oct 24, 2023 •

edited

Loading

KOKIAOKI commented Oct 26, 2023 •

edited

Loading

codecov bot commented Oct 27, 2023 •

edited

Loading